package edu.uhcl.team_drone.drone;

public class StabilizerComponent {

    private Drone owner;
    private PIDController PIDx, PIDz;

    public StabilizerComponent(Drone ownerIn) {
        this.owner = ownerIn;
        PIDx = new PIDController(0.49, 0.15, 0.2);
        PIDx.setSetPoint(0.0);
        PIDz = new PIDController(0.49, 0.15, 0.2);
        PIDz.setSetPoint(0.0);
    }

    public void update(float dt) {
        ///PIDx.setProcessVariable((double) owner.position.x - owner.dx);
        float resultX = (float) PIDx.performPID(dt);

        PIDz.setProcessVariable((double)owner.dz);
        float resultZ = (float) PIDz.performPID(dt);
        //System.out.println(owner.position.x + "  " + owner.position.z);
        System.out.println(owner.dx + " " + owner.dz);
//        System.out.println(resultX + "   " + resultZ);

        if (!owner.inputCmp.isKeyPressed()) {
            owner.up.rotate(owner.direction, resultZ);
            // owner.up.rotate(owner.right, -resultX);
            owner.direction = owner.up.cpy().crs(owner.right).nor();
        }

    }

}
